/********************************************************************************
* This program is free software; you can redistribute it and/or modify it under *
* the terms of the GNU General Public License as published by the Free Software *
* Foundation; either version 2 of the License, or (at your option) any later 	*
* version. This program is distributed in the hope that it will be useful, but 	*
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more *
* details. You should have received a copy of the GNU General Public License 	*
* along with this program; if not, write to the Free Software Foundation, Inc., *
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.						*
* (c) Copyright 2012 José Delgado Pérez, josedelgado@ula.ve josed43@gmail.com.	*				*
********************************************************************************/

/**
 @file percepcion.cpp
 @brief Desarrollo e implementación de los métodos de manejo del sistema de percepción de la plataforma LASDAI LR2
 	 	definidos en la clase PERCEPCIONLR2_H, estos métodos manejan toda la funcionalidad del sistema de percepción, esta clase
 	 	esta parcialmente aislada al hardware si cambiamos el modelo de un sensor solo debemos cambiar el manejador de este sensor
 	 	y los parámetros en el el archivo de definicion.h.

 @date Diciembre, 2012.
 @author José Delgado Pérez, josedelgado@ula.ve josed43@gmail.com.
 @version 0.9
*/

#include "percepcion.h"

/****** Métodos de manejo de la funcionalidad del sistema de percepción de la plataforma LASDAI-LR2 ******/

percepcionLR2::percepcionLR2(){
	sensoresUS[0].setPin(PIN_US0);
	sensoresUS[1].setPin(PIN_US1);
	sensoresUS[2].setPin(PIN_US2);
	sensoresUS[3].setPin(PIN_US3);
	sensoresUS[4].setPin(PIN_US4);
	sensoresUS[5].setPin(PIN_US5);
	sensoresUS[6].setPin(PIN_US6);
	sensoresUS[7].setPin(PIN_US7);
	sensoresUS[8].setPin(PIN_US8);
	sensoresIR[0].setPin(PIN_IR0);
	sensoresIR[1].setPin(PIN_IR1);
	sensoresIR[2].setPin(PIN_IR2);
	sensoresIR[3].setPin(PIN_IR3);
	sensoresIR[4].setPin(PIN_IR4);
	sensoresIR[5].setPin(PIN_IR5);
}

/**************************************************************************************************/

void percepcionLR2::inicializarSistemaPerecepcion(Servo * servo) {
	servoUS = servo;
	sensoresUS[0].setPin(PIN_US0);
	sensoresUS[1].setPin(PIN_US1);
	sensoresUS[2].setPin(PIN_US2);
	sensoresUS[3].setPin(PIN_US3);
	sensoresUS[4].setPin(PIN_US4);
	sensoresUS[5].setPin(PIN_US5);
	sensoresUS[6].setPin(PIN_US6);
	sensoresUS[7].setPin(PIN_US7);
	sensoresUS[8].setPin(PIN_US8);
	sensoresIR[0].setPin(PIN_IR0);
	sensoresIR[1].setPin(PIN_IR1);
	sensoresIR[2].setPin(PIN_IR2);
	sensoresIR[3].setPin(PIN_IR3);
	sensoresIR[4].setPin(PIN_IR4);
	sensoresIR[5].setPin(PIN_IR5);
}

/**************************************************************************************************/

unsigned short int percepcionLR2::obtenerMedidaUS(int idUS) {
	unsigned short int distancia;
	distancia = filtroUS(idUS);
	return distancia;
}

/**************************************************************************************************/

uint8_t percepcionLR2::obtenerMedidaIR(int idIR) {
	uint8_t distancia;
	distancia = filtroIR(idIR);
	return distancia;
}

/**************************************************************************************************/

unsigned short int  percepcionLR2::obtenerMedidaUSTrasero(int angulo) {
	unsigned short int  distancia;
	if(angulo > 130){ angulo = 130; }else{ if(angulo < 50){ angulo = 50; } }
	servoUS->write(angulo);
	//servoUS.escribirServomotor(angulo);
	delay(200);
	distancia = obtenerMedidaUS(8);
	return distancia;
}

/**************************************************************************************************/

unsigned short int* percepcionLR2::obtenerBarridoFrontalUS() {
	static unsigned short int distancias[6];
	for (int i = 0; i < 3; ++i) {
		distancias[i] = obtenerMedidaUS( i + 1 );
		distancias[i + 3] = obtenerMedidaUS( i + 4 );
	}
	return distancias;
}

/**************************************************************************************************/

uint8_t* percepcionLR2::obtenerBarridoFrontalIR() {
	static uint8_t distancias[6];
	for (int i = 0; i < 3; ++i) {
		distancias[i] = obtenerMedidaIR( i + 1 );
		distancias[i + 3] = obtenerMedidaIR( i + 4 );
	}
	return distancias;
}

/**************************************************************************************************/

unsigned short int * percepcionLR2::obtenerBarridoTraseroUS() {
	static unsigned short int distancias[5];
	int angulo = 50;
	servoUS->write(angulo);
    //servoUS.escribirServomotor(angulo);
	delay(200);
	for (int i = 0; i < 5; ++i) {
		distancias[i] = obtenerMedidaUS(8);
		angulo = angulo + 20;
		//servoUS.escribirServomotor(angulo);
		servoUS->write(angulo);
		delay(45);
	}
	//servoUS.escribirServomotor(90);
	servoUS->write(90);
	return distancias;
}

/**************************************************************************************************/

percepcionLR2::~percepcionLR2() {
	servoUS->detach();
	for (int var = 0; var < 8; ++var) {
		sensoresUS[var].~sensorUS();
	}
	for (int var = 0; var < 5; ++var) {
		sensoresIR[var].~sensorIR();
	}
}

/**************************************************************************************************/

unsigned short int percepcionLR2::filtroUS(int idUS) {
	int vector[SIZE_FILTRO_US], mediana, medidaPulgadas, medidaCm;
	for( int i = 0 ; i < SIZE_FILTRO_US ; i++ ){
		vector[i] = sensoresUS[idUS].obtenerLectura();
		delay(TIEMPO_MUESTREO_US);
	}
	for( int i = 0 ; i < SIZE_FILTRO_US ; i++ ){
		int j = vector[i], k;
		for( k = i - 1 ; (k >= 0) && (j < vector[k]) ; k-- ){
			vector[ k + 1 ] = vector[ k ];
		}
		vector[ k + 1 ] = j;
	}
	mediana = (int)(SIZE_FILTRO_US/2);
	medidaPulgadas = (int)(vector[mediana]/2);
	medidaCm = (unsigned short int)( medidaPulgadas * 2.54 );
	return medidaCm;
}

/**************************************************************************************************/

uint8_t percepcionLR2::filtroIR(int idIR) {
	    float voltios, distancia;
	    int mediana;
	    float vector[SIZE_FILTRO_IR];
	    for( int i = 0 ; i < SIZE_FILTRO_IR ; i++){
	        vector[i] = sensoresIR[idIR].obtenerLectura();
	        delay(TIEMPO_MUESTREO_IR);
	    }
	    for( int i = 0 ; i < SIZE_FILTRO_IR ; i++ ){
	        int j = vector[i], k;
	        for( k = i - 1 ; (k >= 0) && (j < vector[k]) ; k-- ){
	            vector[ k + 1 ] = vector[ k ];
	        }
	        vector[ k + 1 ] = j;
	    }
	    mediana = (int)(SIZE_FILTRO_IR/2);
	    voltios = vector[mediana]*RESOLUCION_IR;
	    if( (voltios >= 0.4) && (voltios < 2.7) ){
	    	distancia = ((16.75*pow(voltios,4.0))-(119.26*pow(voltios,3.0))+(311.7*pow(voltios,2.0))-(365.71*voltios)+184.03);
	    }else{
	    	if(voltios < 0.4){
	    		distancia = 0;
	    	}else{
	    		distancia = 80;
	    	}
	    }
	    return (uint8_t)distancia;
}
